home *** CD-ROM | disk | FTP | other *** search
/ HPAVC / HPAVC CD-ROM.iso / ZED3DSRC.ZIP / XFORMS.C < prev    next >
C/C++ Source or Header  |  1995-06-19  |  8KB  |  446 lines
  1. #include <xforms.h>
  2. #include <stdio.h>
  3.  
  4.  
  5.  
  6. /* if your compiler supports some form of function inlining, you can try
  7.    removing the comment below (doubt it will work though) */
  8.  
  9. /* For the two next functions, in a real program they would probably
  10.    be implemented as macros for more speed */
  11. fixedpoint fixed_mul(fixedpoint a, fixedpoint b)
  12.     {
  13.     #ifdef __TURBOC__
  14.     #pragma inline
  15.  
  16.     /* Note:
  17.         The following asm code is very fast for a PC. Ideally, you should
  18.         make it so that you use this code instead of the code after the
  19.         #else (of course, ideally it should be inlined...) */
  20.  
  21.     long c;
  22.     asm{
  23.         mov eax,a /*first operand*/
  24.         mov edx,b /*second operand*/
  25.         imul edx  /*multiply them*/
  26.         shl edx,16 /* put integer value in upper 16 bits*/
  27.         shr eax,16 /* truncate fraction part*/
  28.         or eax,edx /* fuse integer and fractional part*/
  29.         mov c,eax
  30.         }
  31.     return c;
  32.     #else
  33.     return ((a/256)*(b/256)); /* a poor patch*/
  34.     #endif
  35.     }
  36.  
  37.  
  38.  
  39. fixedpoint fixed_div(fixedpoint a, fixedpoint b)
  40.     {
  41.     #ifdef __TURBOC__
  42.     /* Note:
  43.         The following asm code is very fast for a PC. Ideally, you should
  44.         make it so that you use this code instead of the code after the
  45.         #else (of course, ideally it should be inlined...) */
  46.     long c;
  47.     asm{
  48.         mov edx,a /*load a in edx:eax, 64 bits*/
  49.         mov eax,edx
  50.         sar edx,16
  51.         shl eax,16
  52.         mov ecx,b /* load b in ecx*/
  53.         idiv ecx /* divide edx:eax by ecx*/
  54.         mov c,eax
  55.         }
  56.     return c;
  57.     #else
  58.     return ((a*256)/(b/256)); /* poor patch*/
  59.     #endif
  60.     }
  61.  
  62.  
  63. void mat_add(matrix r, matrix a, matrix b)
  64.     {
  65.     int x,y;
  66.  
  67.     for(x=0;x<3;x++)
  68.         for(y=0;y<3;y++)
  69.             r[x][y]=add(a[x][y],b[x][y]);
  70.     }
  71.  
  72.  
  73.  
  74. void mat_sub(matrix r, matrix a, matrix b)
  75.     {
  76.     int x,y;
  77.  
  78.     for(x=0;x<3;x++)
  79.         for(y=0;y<3;y++)
  80.             r[x][y]=sub(a[x][y],b[x][y]);
  81.     }
  82.  
  83.  
  84.  
  85. void mat_mul(matrix r, matrix a, matrix b)
  86.     {
  87.     int x,y,z;
  88.  
  89.     for(x=0;x<3;x++)
  90.         for(y=0;y<3;y++)
  91.             {
  92.             r[y][x]=floattoreal(0.0);
  93.             for(z=0;z<3;z++)
  94.                 r[y][x]=add(r[y][x],mul(a[z][x],b[y][z]));
  95.             }
  96.     }
  97.  
  98.  
  99.  
  100. void mat_mul_vec(vector r, matrix a, vector b)
  101.     {
  102.     int x,y;
  103.  
  104.     for(x=0;x<3;x++)
  105.         {
  106.         r[x]=floattoreal(0.0);
  107.         for(y=0;y<3;y++)
  108.             r[x]=add(r[x],mul(a[y][x],b[y]));
  109.         }
  110.     }
  111.  
  112.  
  113.  
  114. void vec_add(vector r, vector a, vector b)
  115.     {
  116.     int x;
  117.  
  118.     for(x=0;x<3;x++)
  119.         r[x]=add(a[x],b[x]);
  120.     }
  121.  
  122.  
  123.  
  124.  
  125. void vec_sub(vector r, vector a, vector b)
  126.     {
  127.     int x;
  128.  
  129.     for(x=0;x<3;x++)
  130.         r[x]=sub(a[x],b[x]);
  131.     }
  132.  
  133.  
  134.  
  135.  
  136. REAL vec_dot(vector a, vector b)
  137.     {
  138.     /* a dot b is a[0]*b[0]+a[1]*b[1]+a[2]*b[2] */
  139.     REAL dot;
  140.     int x;
  141.  
  142.     dot=floattoreal(0.0);
  143.     for(x=0;x<3;x++)
  144.         dot=add(dot,mul(a[x],b[x]));
  145.     return dot;
  146.     }
  147.  
  148.  
  149.  
  150. void vec_normalize(vector a)
  151.     {
  152.     REAL length;
  153.     int x;
  154.  
  155.     /* first use find euclidian length of vector. It is the square root
  156.        of the sum of the square of the vector components. E.g. in 2d, this
  157.        simplifies to pythagora's theorem, a^2+b^2=c^2. In 3d, it's
  158.        a^2+b^2+c^2=d^2 */
  159.  
  160. #ifdef use_fixed
  161.     do {
  162. #endif
  163.     length=floattoreal(0.0);
  164.  
  165.     for(x=0;x<3;x++)
  166.         length=add(length,mul(a[x],a[x])); /* calculate sum */
  167.  
  168.     length=SQRT(length); /* extract square root */
  169. #ifdef use_fixed
  170.     if(length<=floattoreal(0.001))
  171.         vec_mul_scl(a,a,floattoreal(1000));
  172.     } while(length<=floattoreal(0.001));
  173. #endif
  174.  
  175.     /* second divide each component of the vector by the length of the
  176.        vector */
  177.     for(x=0;x<3;x++)
  178.         a[x]=div(a[x],length);
  179.     }
  180.  
  181.  
  182.  
  183. void vec_mul_scl(vector r, vector a, REAL b)
  184.     {
  185.     int x;
  186.  
  187.     for(x=0;x<3;x++)
  188.         r[x]=mul(a[x],b);
  189.     }
  190.  
  191.  
  192.  
  193. void vec_crs(vector r, vector a, vector b)
  194.     {
  195.     /* (a0,a1,a2) cross (b0,b1,b2) is
  196.        (a1b2-a2b1 , a2b0,a0b2 , a0b1-a1b0) */
  197.     r[0]=sub(mul(a[1],b[2]),mul(a[2],b[1]));
  198.     r[1]=sub(mul(a[2],b[0]),mul(a[0],b[2]));
  199.     r[2]=sub(mul(a[0],b[1]),mul(a[1],b[0]));
  200.     }
  201.  
  202.  
  203.  
  204.  
  205. void mat_orthonormalize(matrix a)
  206.     {
  207.     vector temp;
  208.     /* normalize first vector */
  209.     vec_normalize(a[0]);
  210.  
  211.     /* make second vector perpendicular to the first */
  212.     vec_mul_scl(temp,
  213.                 a[0],
  214.                 vec_dot(a[1],a[0])
  215.                 );
  216.     vec_sub(a[1],
  217.             a[1],
  218.             temp
  219.             );
  220.  
  221.     /* normalize second vector */
  222.     vec_normalize(a[1]);
  223.  
  224.     /* third vector is first cross second */
  225.     vec_crs(a[2],a[0],a[1]);
  226.     /* no need to normalize this third one since the first two are
  227.        normalized*/
  228.     }
  229.  
  230.  
  231. #define z(x,y) m[x][y]=floattoreal(a##y##x)
  232.  
  233. void initmatrix(matrix m,
  234.     float a00, float a01, float a02,
  235.     float a10, float a11, float a12,
  236.     float a20, float a21, float a22)
  237.     {
  238.     z(0,0);
  239.     z(0,1);
  240.     z(0,2);
  241.  
  242.     z(1,0);
  243.     z(1,1);
  244.     z(1,2);
  245.  
  246.     z(2,0);
  247.     z(2,1);
  248.     z(2,2);
  249.     }
  250.  
  251. void printmatrix(matrix m)
  252.     {
  253.     int x;
  254.  
  255.     for(x=0;x<3;x++)
  256.         {
  257.         printf("\n%13.5f %13.5f %13.5f",
  258.                realtofloat(m[0][x]),
  259.                realtofloat(m[1][x]),
  260.                realtofloat(m[2][x])
  261.                );
  262.         }
  263.  
  264.     printf("\n");
  265.     }
  266.  
  267.  
  268.  
  269. void initvector(vector v,
  270.     float i, float j, float k)
  271.     {
  272.     v[0]=i;
  273.     v[1]=j;
  274.     v[2]=k;
  275.     }
  276.  
  277.  
  278.  
  279.  
  280. void printvector(vector v)
  281.     {
  282.     printf("\n(%f,%f,%f)\n",
  283.         realtofloat(v[0]),
  284.         realtofloat(v[1]),
  285.         realtofloat(v[2]));
  286.     }
  287.  
  288.  
  289.  
  290.  
  291. void copymatrix(matrix dest, matrix src)
  292.     {
  293.     copyvector(dest[0],src[0]);
  294.     copyvector(dest[1],src[1]);
  295.     copyvector(dest[2],src[2]);
  296.     }
  297.  
  298.  
  299.  
  300. void copyvector(vector dest, vector src)
  301.     {
  302.     dest[0]=src[0];
  303.     dest[1]=src[1];
  304.     dest[2]=src[2];
  305.     }
  306.  
  307.  
  308.  
  309. void rotatevectors(vector v1, vector v2,
  310.                    vector u1, vector u2,
  311.                    REAL theta)
  312.     {
  313.     REAL s,c; /* sintheta and costheta respectively */
  314.     vector t1,t2,g1,g2;
  315.  
  316.     s=SIN(theta);
  317.     c=COS(theta);
  318.  
  319.     /* formula is: u1=v1costheta-v2sintheta
  320.                    u2=v1sintheta+v2costheta */
  321.  
  322.     vec_mul_scl(t1,v1,c);
  323.     vec_mul_scl(t2,v2,s);
  324.  
  325.     vec_mul_scl(g1,v1,s);
  326.     vec_mul_scl(g2,v2,c);
  327.  
  328.     vec_sub(u1,t1,t2);
  329.     vec_add(u2,g1,g2);
  330.     }
  331.  
  332.  
  333.  
  334. void rotatebase(matrix m, int axis, REAL theta)
  335.     {
  336.     int a,b;
  337.  
  338.     a=axis+1;
  339.     if(a>=3)
  340.         a-=3;
  341.     b=a+1;
  342.     if(b>=3)
  343.         b-=3;
  344.  
  345.     rotatevectors(m[a],m[b],m[a],m[b],theta);
  346.     }
  347.  
  348.  
  349. REAL determinant(matrix m)
  350.     {
  351.     /* det m is:
  352.        m[0][0]*m[1][1]*m[2][2]+
  353.        m[0][1]*m[1][2]*m[2][0]+
  354.        m[0][2]*m[1][0]*m[2][1]
  355.        -
  356.        m[0][0]*m[1][2]*m[2][1]-
  357.        m[0][1]*m[1][0]*m[2][2]-
  358.        m[0][2]*m[1][1]*m[2][0]
  359.        */
  360.  
  361.     return
  362.         mul(mul(m[0][0],m[1][1]),m[2][2])+
  363.         mul(mul(m[0][1],m[1][2]),m[2][0])+
  364.         mul(mul(m[0][2],m[1][0]),m[2][1])
  365.         -
  366.         mul(mul(m[0][0],m[1][2]),m[2][1])-
  367.         mul(mul(m[0][1],m[1][0]),m[2][2])-
  368.         mul(mul(m[0][2],m[1][1]),m[2][0]);
  369.     }
  370.  
  371.  
  372. void mat_mul_scl(matrix r, matrix a, REAL b)
  373.     {
  374.     int x,y;
  375.  
  376.     for(x=0;x<3;x++)
  377.         for(y=0;y<3;y++)
  378.             r[x][y]=mul(a[x][y],b);
  379.     }
  380.  
  381.  
  382.  
  383. int invert(matrix m, matrix r)
  384.     {
  385.     /* inverse matrix is cofactors matrix divided by determinant if
  386.        determinant is nonzero. so first calculate determinant, if
  387.        zero return error. then calculate cofactor matrix and divide by
  388.        determinant */
  389.     matrix cof;
  390.     REAL det;
  391.  
  392.  
  393.     det=determinant(m);
  394.     if(!det)
  395.         {
  396.         return -1; /* error, matrix inverse does not exist */
  397.         }
  398.  
  399.     /* matrix of cofactors is hard wired here for 3x3 matrices */
  400.     initmatrix(r,
  401.         mul(m[1][1],m[2][2])-mul(m[1][2],m[2][1]),
  402.         mul(m[2][0],m[1][2])-mul(m[1][0],m[2][2]),
  403.         mul(m[1][0],m[2][1])-mul(m[2][0],m[1][1]),
  404.  
  405.         mul(m[2][1],m[0][2])-mul(m[0][1],m[2][2]),
  406.         mul(m[0][0],m[2][2])-mul(m[0][2],m[2][0]),
  407.         mul(m[2][0],m[0][1])-mul(m[0][0],m[2][1]),
  408.  
  409.         mul(m[0][1],m[1][2])-mul(m[1][1],m[0][2]),
  410.         mul(m[1][0],m[0][2])-mul(m[0][0],m[1][2]),
  411.         mul(m[0][0],m[1][1])-mul(m[1][0],m[0][1]));
  412.  
  413.     /* now we have matrix of cofactors. multiply it by 1/det */
  414.  
  415.     det=div(floattoreal(1.0),det);
  416.  
  417.     mat_mul_scl(r,r,det);
  418.  
  419.     /* return success */
  420.     return 0;
  421.     }
  422.  
  423.  
  424.  
  425. int invert_affine(affine *a, affine *r)
  426.     {
  427.     if(invert(a->m,r->m))
  428.         return -1;
  429.  
  430.     mat_mul_vec(r->v,a->m,a->v);
  431.     r->v[0]=-r->v[0];
  432.     r->v[1]=-r->v[1];
  433.     r->v[2]=-r->v[2];
  434.     return 0;
  435.     }
  436.  
  437.  
  438.  
  439.  
  440. void affine_xform(affine *a, vector i, vector j)
  441.     {
  442.     mat_mul_vec(j,a->m,i);
  443.     vec_add(j,j,a->v);
  444.     }
  445.  
  446.